Skip to content

Control v2 - #291

Open
MatthieuDarcy wants to merge 26 commits into
mainfrom
control_v2
Open

Control v2#291
MatthieuDarcy wants to merge 26 commits into
mainfrom
control_v2

Conversation

@MatthieuDarcy

Copy link
Copy Markdown
Contributor

This PR adds at first attempt at support for closed loop control in dynastyx.

Added in /dynestyx/control

  1. DiscreteControlLoopSimulator

API:

    def model():
        with DiscreteControlLoopSimulator(
            control_policy=policy,
            policy_state_init=None,
            filter_config=FilterConfig(record_filtered_states_mean=True),
        ):
            return dsx.sample("loop", dynamics, predict_times=predict_times)
  1. MPPI

Basic MPPI controller

API

mppi = MPPI(
    dynamics_model=dynamic_rollout
    loss_fn=quadratic_loss,
    horizon=horizon,
    n_samples=n_samples,
    noise_std=1.0,
    temperature=1.0,
)

Added in tutorials
3. Notebook controller_demo.ipynb

Additional elements I will work on

  1. Verifying compatibility with different filters.
  2. Verifying gradient computation.

Closes #288

MatthieuDarcy and others added 11 commits July 30, 2026 12:09
Implements the discrete-time control loop from the design issue: a new
DiscreteControlLoopSimulator (alongside DiscreteTimeSimulator) interleaves
simulation, observation, filtering, and control online, deciding each u_k
from the filtered belief via a user-supplied policy rather than requiring
the whole control trajectory up front.

FilterUpdate is powered by a new compute_cuthbert_filter_update, which
drives cuthbert's existing Filter.filter_prepare/filter_combine primitives
one step at a time instead of over a whole pre-supplied trajectory --
generic across KFConfig/EKFConfig/EnKFConfig/PFConfig. Verified PFConfig and
EnKFConfig support genuinely black-box state transitions (no log_prob
required), matching the design issue's black-box dynamics requirement.

Includes a demo notebook (docs/tutorials/control/controller_demo.ipynb)
showing a linear feedback policy driving a 1D linear-Gaussian system to 0.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
…transitions

Adds a tutorial section demonstrating a genuinely continuous-time SDE
(ContinuousTimeStateEvolution + Discretizer(discretize=euler_maruyama))
composed with DiscreteControlLoopSimulator, showing the loop only ever sees
discrete times while the underlying dynamics are a true SDE solved between
them.

Building this surfaced a real bug: the bootstrap FilterUpdate call left
t_prev defaulting to t (dt=0). For fixed-covariance transitions this is
harmless, but for a dt-scaled transition (like the Euler-Maruyama
discretization here), it constructs a zero-covariance distribution whose
NaN log-density leaks through the gradient in EKF's Taylor linearization
(jnp.where evaluates both branches, unlike jax.lax.cond) -- corrupting the
filtered state and, downstream, the policy's control and the simulated
trajectory itself. Fixed by giving the bootstrap call a non-degenerate
t_prev, borrowing the width of the first real interval (matching
compute_cuthbert_filter's own dummy-row convention).

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
…ccuracy

Documents how the continuous-time integration is actually defined: Discretizer
takes exactly one Euler-Maruyama step over the whole gap between requested
times (needed so filtering gets an explicit one-step transition density),
unlike SDESimulator/solve_sde's substepped solvers, which only support pure
forward simulation without filtering.

Adds a section 6 demonstrating the same recipe on a genuinely nonlinear 2D
system with drift = A x^2 + u: x=0 is an unstable equilibrium (x^2 >= 0 always
pushes away from the origin), so the uncontrolled run diverges while the same
linear feedback policy as before stabilizes it locally. Uses a finer time grid
(dt=0.1 vs 1.0) since the one-step EM approximation needs a small enough gap
to stay accurate for a nonlinear drift.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
…rt_filter_update

35 tests across 7 groups: filter_state_mean unit tests, compute_cuthbert_filter_update
correctness (including regression tests for the dt=0 EKF NaN bug), validation/error
paths, end-to-end shape/output-key tests (including the stateless-policy regression),
behavioral/control correctness (closed-loop stabilization, the control-index
convention, determinism, eqx.Module policies), continuous-time/Discretizer
composition, and black-box transition compatibility.

Surfaced one more real gap while writing these: a genuinely nested-pytree
policy_state_init (e.g. a dict of arrays) can't actually round-trip today --
BaseSimulator's shared _run_single_member_simulation enforces a
dict[str, Array] | None return type via jaxtyping at runtime, so
policy_states must stay a flat Array. Documented in the corresponding test
rather than widening the shared base class's type contract, which is out of
scope here.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
…ulator there

Consolidates control-loop + control-policy code under dynestyx/control/:
moves discrete_controller_simulators.py there, and adds a basic MPPI
(Model Predictive Path Integral) controller (dynestyx/control/mppi.py) that
plugs into the same control_policy= slot. Deliberately simple: samples
candidate control sequences as Gaussian perturbations around a nominal
sequence, rolls each through a user-supplied dynamics_model, and returns
the softmax-weighted mean control (standard MPPI weighting). dynamics_model
can be batched (one call handles all samples) or not (driven via
jax.lax.map, so it never needs to support batching itself).

MPPI needs fresh randomness every step, which PolicyCallable's signature
didn't provide -- added a key parameter to __call__(x_hat, s, key), passed
in by DiscreteControlLoopSimulator's per-step scan. This also keeps MPPI's
own policy state a flat array (just the nominal control sequence), avoiding
a real limitation found while testing last time: a nested-pytree
policy_state_init can't actually be recorded as output today, since
BaseSimulator's shared return-type check requires flat Array values.

Verified: existing linear-feedback policies (tests, tutorial notebook)
updated for the new signature, full regression suite still passes
(117 tests), and a smoke test confirms MPPI stabilizes a marginally-unstable
linear system identically under both batched and non-batched dynamics_model.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
# Conflicts:
#	dynestyx/inference/integrations/cuthbert/discrete_filter.py
…lation)

main/control_v2 landed a major refactor (commit 3fb1cd7, "Numpyro-Free
Usage") that deleted dynestyx/simulators.py entirely, replacing it with a
new dynestyx/simulation/ package: BaseSimulator subclasses now implement a
public `simulate(dynamics, *, rng_key, ...) -> SimulatedResult` (not
`_simulate(name, ..., obs_times=..., ...) -> dict`), take rng_key directly
instead of calling numpyro.prng_key() internally, and numpyro site
registration happens via a generic deferred callback (dataclasses.fields()
iteration over the returned SimulatedResult) rather than base-class magic
over a free-form dict.

Ported DiscreteControlLoopSimulator to the new contract:
- Plain class with explicit __init__ (matching DiscreteTimeSimulator's new
  shape) instead of a bare @dataclasses.dataclass.
- New ControlledSimulatedResult(SimulatedResult) subclass carrying the
  extra controls/filtered_states_mean/policy_states fields -- confirmed by
  reading base.py directly that _run_single_member_simulation's common path
  is a bare `return self.simulate(...)` with no reconstruction, so subclass
  fields flow through untouched and get registered generically the same way
  as SimulatedResult's own fields (None-valued fields are auto-skipped).
- Dropped the now-redundant manual `numpyro.prng_key()`/obs_values checks:
  the base class already validates/rejects these before simulate() is ever
  called.

compute_cuthbert_filter_update/_build_cuthbert_filter_obj (added to
discrete_filter.py on the `control` branch) merged in via `git merge control`
with a single trivial conflict (re-adding the `jax.random` import) --
confirmed via git merge-tree dry run and direct diffing that every symbol
they depend on exists byte-identical post-refactor, just re-imported from
dynestyx.inference.configs.filter instead of dynestyx.inference.filter_configs.
Discretizer required no changes at all (already ported upstream).

Verified: full regression suite passes on control_v2 (118 tests: this
file's 35 plus test_filters.py/test_filter_simulator.py/test_discretizers.py/
test_predictive_filter_simulator_shapes.py), the tutorial notebook
re-executes cleanly end-to-end, and MPPI produces numerically identical
results to its control-branch run under both batched and non-batched
dynamics_model.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Section 7 demonstrates dynestyx.control.MPPI plugged into the same
control_policy= slot as the earlier linear-feedback policy, on the same 1D
system from section 1. Builds a planning rollout directly from
dynamics.state_evolution's deterministic mean (a standard MPPI
simplification -- the planner doesn't need a faithful stochastic
simulation, just a reasonable prediction of where a candidate control
sequence leads) and a simple quadratic loss, then compares against the
K=0 no-control baseline already computed in section 3.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
DynamicalModel.state_evolution is declared as the broader
ContinuousTimeStateEvolution | DiscreteStateTransition union, so `ty`
couldn't statically narrow continuous_dynamics.state_evolution to
StochasticContinuousTimeStateEvolution before passing it to
euler_maruyama() (which requires the narrower type) -- even though this is
exactly how Discretizer._sample_ds itself resolves it at runtime. Added the
same isinstance narrowing check used there.

Also includes an incidental ruff-format tweak to mppi.py (collapsing a
one-line call that had been wrapped unnecessarily).

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
@MatthieuDarcy

Copy link
Copy Markdown
Contributor Author

The biggest addition to other parts of the code is in cuthbert/discrete_filter.py

compute_cuthbert_filter_update now computes a one step filtering update: takes in the previous estimated state, a control, and observation and provides a new estimated state.

…book

Section 8 demonstrates that DiscreteControlLoopSimulator is differentiable
end-to-end: rolls out a short horizon with the linear policy from section 2,
defines a loss as the norm of the final true state, and takes jax.grad of
it with respect to the gain K -- no special machinery needed beyond plain
JAX autodiff. Verified independently against a finite-difference check
before writing the notebook cells (gradient matched to ~1e-4), and shows
one gradient-descent step meaningfully reducing the loss (0.62 -> 0.18).

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>

@mattlevine22 mattlevine22 left a comment

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks great, thank you @MatthieuDarcy !

  1. Can we have @cooijmanstim look at the Jupyter notebook demo, and get his take?

  2. My preliminary comments/questions (haven't looked carefully through the code yet):

  • I think DiscreteControlLoopSimulator should be made available directly through the Simulator() handler via simulation/auto.py (and, hence, numpyro-free dsx.simulate) interface.
  • Should check that we can autodiff through a ClosedLoopSimulation.
  • Should probably support n_simulations > 1.
    -DiscreteControlLoopSimulator should another argument for an "approximate model" (assuming the "true" model is in the dsx.sample statement)?
  • I like that one can feed very generic controllers (including MPPI); however, I think the current MPPI setup should be better curated so that the user doesn't have to do a bunch of the extra manual work that appears in the demo. I think the following should be possible:
sim_mppi = DiscreteControlLoopSimulator(
    control_policy_config=MPPIConfig(
                                                      loss_fn="quadratic",  # or a callable
                                                      horizon=horizon,
                                                      n_samples=n_samples,
                                                      noise_std=1.0, # is this needed?
                                                      temperature=1.0,
                                                ),
    policy_state_init=mppi_initial_state(horizon, control_dim),
    filter_config=KFConfig(record_filtered_states_mean=True),
)

# MPPI should have make_mppi_rollout internally (don't expose user to it)

@cooijmanstim

Copy link
Copy Markdown
Contributor

Looks great, this interface works for us. For the notebook it would be good besides no control vs control to see the effectiveness of the controller as the noise level on the observations changes. Just three variants in total will do (no control, control with lots of noise, control with little noise).

MatthieuDarcy and others added 7 commits July 31, 2026 09:11
Section 3 (differentiating through the closed loop) computed grad_K via
jax.grad but never independently verified it. Added a cell checking a
central finite difference on K[0, 0] against the autodiff value -- confirms
the gradient through the whole closed loop (policy -> transition ->
observation -> filter update) is actually correct, not just plausible.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Section 3's rollout_final_state_norm now calls sim.simulate(dynamics,
rng_key=..., predict_times=...) directly instead of going through
dsx.sample/numpyro.handlers.seed -- unneeded here since there's no
conditioning/MCMC, and it makes the PRNG key an explicit argument instead
of an implicit fixed rng_seed=0.

More importantly, this fixes a real issue in section 4's training loop:
every epoch previously reused the same fixed seed, so gradient descent could
overfit K to one specific noise realization rather than the underlying
dynamics. Now each epoch draws a fresh key via jax.random.split, so K_opt
has to generalize across realizations. Verified the loop still converges to
a sensible stabilizing gain with this fresh-key-per-epoch setup before
editing the notebook. Section 4's final comparison plot (run()/dsx.sample)
is left unchanged -- there, reusing the same seed for both the unoptimized
and optimized rollouts is what makes the before/after comparison fair.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
run() (updated separately) now returns a ControlledSimulatedResult from
sim.simulate() directly instead of a numpyro trace dict, so the plotting
cell needed the equivalent attribute-access update: trace["loop_X"]["value"]
-> result.X. Also removed a leftover duplicate plotting cell still using the
old trace-dict access, which was left after the updated one and caused the
notebook to fail on execution.

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Replaces the with sim: dsx.sample(...) + numpyro.handlers.seed/trace pattern
across every section (discrete demo, SDE, nonlinear 2D, MPPI) with direct
sim.simulate(dynamics, rng_key=key, predict_times=...) calls returning
ControlledSimulatedResult objects -- no NumPyro handler or model function
needed, since every section here is a pure generative rollout with nothing
to condition on. Renamed trace_* variables to result_* throughout, since
they're no longer NumPyro trace dicts, and updated all downstream plotting
cells from trace["name_field"]["value"] to result.field attribute access.

For the continuous-time sections, this also removes the Discretizer handler
composition: instead of wrapping dsx.sample in `with Discretizer(...)`, the
continuous-time DynamicalModel is discretized once up front via
euler_maruyama(state_evolution) into a plain discrete-time DynamicalModel,
which is then simulated exactly like any other discrete-time model. Updated
the surrounding markdown to match (no more handler-chain explanation needed).

Co-Authored-By: Claude Sonnet 5 <noreply@anthropic.com>
Unified non-linear dynamics, black box and partial observations example

@DanWaxman DanWaxman left a comment

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks like a good start! Some early high-level comments.

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Some notes*:

  • $x_hat_{k|k}$ in the control policy should be $\hat{x}_{k|k}$.
  • "with $a=1.05$, is linear with additive Gaussian noise. Without control, this system is unstable and $x_k \rightarrow \infty$" depends on the initial condition (if $x_0 &lt; 0$, then $x_k \to -\infty$).
  • We should probably package a few things, e.g., linear policy, so that the user does not have to recreate it in the simplest cases
  • I think I would prefer if we follow the more general dynestyx pattern of using with DiscreteControlLoopSimulator(): dsx.sample(...), though what you wrote also works
  • What does $x_t^2$ mean in the SDE example (I think $x_t \odot x_t$?)
  • "Both _cuthbert_filter_pf and _cuthbert_filter_enkf only call dynamics.state_evolution(...).sample(key)" this is more internal than I'd want to include in a tutorial
  • MPPI is enough of a jump that it might be worth making a genuinely follow-up notebook

* I understand a lot of this was probably preliminary text anyways, just figured it might be useful since a few of them are more general patterns

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Would it make more sense to do LQR here?

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I will take a look at some implementation of LQR, but I actually think MPPI is the better first example (typically more robust and works better on a broader range of problems for my understanding).

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sounds good! I think if the example is a linear policy on a linear SSM, might as well do the LQR loss function, but doing MPPI as the main tutorial is also fine by me.

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it might be beneficial to just implement these directly in Simulator classes. The issue here is just that we made a split to actively recommend against Simulator for parameter inference...

Comment on lines +218 to +247
r"""One-step FilterUpdate: state_k + u_k + y_{k+1} -> state_{k+1}.

Unlike `compute_cuthbert_filter` (whole-trajectory), this performs exactly
one predict+update step using cuthbert's `Filter.filter_prepare`/
`filter_combine` primitives directly, without requiring future
observations. This is what makes online control possible: the state
returned here can be consumed by a policy to choose the next control
before the next observation exists.

Pass `prev_state=None` for the bootstrap call (computing the filtering
state after only the first observation, with no control history yet);
this internally calls the cuthbert filter's `init_prepare` first.

Control convention (important, and different from `compute_cuthbert_filter`
/ `DiscreteTimeSimulator`): `u` is the control that drove the transition
*into* the state being filtered, i.e. u_k when producing state_{k+1} from
state_k and y_{k+1} -- matching `FilterUpdate(x_hat_k, u_k, y_{k+1}, ...)`
in the control-loop equations. `compute_cuthbert_filter`/
`DiscreteTimeSimulator` instead pair `ctrl_values[t]` with *both* the
observation and the outgoing transition at the same index t, which is
only valid when the whole control trajectory is already known in advance.
For online control this is impossible: u_{k+1} cannot exist before
y_{k+1} is observed, since it is computed by the policy from the filtered
state that itself depends on y_{k+1}. So `u` here is used for both
`CuthbertInputs.u` and `CuthbertInputs.u_prev` in the single-row input
built for this step. Pass `u=None` for the bootstrap call, matching y_0's
lack of a control argument in the control-loop equations (numerically
equivalent to zeros for models with a control-input matrix, since D=None
or u=None are both treated as "no control contribution").
"""

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

too much docstring for an internal function

Comment on lines +207 to +217
def compute_cuthbert_filter_update(
dynamics: DynamicalModel,
filter_config: BaseFilterConfig,
prev_state,
key: jax.Array,
*,
y: jax.Array,
u: jax.Array | None,
t: jax.Array,
t_prev: jax.Array | None = None,
):

Copy link
Copy Markdown
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can probably change this a bit and then scan over it instead of duplicating prepare/combine code?

Unify simulate API with control_policy/filter_config; add distribution-returning policies; expand controller tutorial

- dsx.simulate() and Simulator now accept control_policy/filter_config,
  routing to DiscreteControlLoopSimulator the same way dynamics.state_evolution
  auto-routes to SDESimulator/ODESimulator/DiscreteTimeSimulator.
- A policy may return a NumPyro
  Distribution instead of a value, sampled automatically; MPPI now owns and
  advances its own exploration randomness via a `seed` field,
- Policies self-initialize via an optional initial_state() (replacing the
  removed policy_state_init argument), mirroring optax's optimizer.init().
- controller_demo.ipynb: sections now use dsx.simulate() directly; added a
  new random-controller example (Section 5) demonstrating a
  distribution-returning policy. unchanged on the nonlinear 2D
  black-box SDE (Section 6). Removed the MPPI example from the basic tutorial (now lives in a separate tutorial.
Rework MPPI to take one-step dynamics; complete and stabilize mpc_demo

- MPPI now takes dynamics: DynamicalModel (one-step transition) instead of
  a hand-rolled full-rollout function; it builds the horizon-unrolling and
  n_samples batching internally via jax.lax.scan/vmap, using .mean when
  available and falling back to .sample() (own internal key) for black-box
  transitions.
- All fields but dynamics/loss_fn now have defaults (horizon=10,
  noise_std=1.0, n_samples=20, dt=1.0, temperature=1.0, batched=True,
  seed=0).
- stability: Non-finite (nan/inf) rollout losses are clamped before the softmax
  weighting, fixing NaN control
- mpc_demo.ipynb: completed (previously referenced undefined names, never
  ran), updated to the new MPPI API, added MPC/MPPI citations.
@MatthieuDarcy

MatthieuDarcy commented Aug 3, 2026

Copy link
Copy Markdown
Contributor Author

Addressed some of the points made (thanks @mattlevine22 and @DanWaxman !).

Changes

  1. DiscreteControlLoopSimulator now works with dsx.simulate
    Example syntax:

dsx.simulate( dynamics, rng_key=key, predict_times=predict_times, control_policy=policy, filter_config=KFConfig(record_filtered_states_mean=True) )

  1. Policy is any callable that takes in x_hat (estimated filtered state) and s (internal state) and returns u and s_new.

u can be an array or a numpyro distribution, in which case the simulator will draw a sample from the distribution.

3. MPPI is more robust and has better defaults. It's only required parameters at initialization are loss_function and dynamics (thus allowing for dynamics different from the dynamics evolving the state itself).

  1. The notebooks controller_demo.ipynb and mpc_demo.ipynbhave been improved and now expose some of these choices. The MPPI example is now exclusively in mpc_demo.ipynb.

Explained in the notebooks are:

  • how to define a policy
  • how to use DiscreteControlLoopSimulator with non-linear dynamics
  • how to use ``MPPI` (highlighting that the dynamics used by MPPI have to be defined outside the simulator and thus can be different from the ones used by the simulator).

Outstanding questions

Currently policy does not use time $t$. This actually might be relevant (such as MPC combined with time dependent dynamics). Should we have policies take 3 arguments instead?

The simulator needs an initial state $s_0$. Internally, the simulator will check if policy has an initial_state attribute, defaulting to None otherwise. This makes it more convenient for the user to implement simple policies. In the notebook, I have the policy explicitly return None to highlight that, in general, policies might need to return an initial state. Is this alright?

Todos

  1. Address some of @DanWaxman's requested changes, especially improving the internal docs.
  2. Simplify the test suite.
  3. Test gradients and policy optimization.
  4. Expand the policies we package.

Update the policy to now take in state, t_now, t_next, s.

Changed how stochastic policies operate: sample within the policy, do not return a distribution. If a distribution is returned, raise an error to indicate to the user that they should sample from it instead.
@MatthieuDarcy

MatthieuDarcy commented Aug 3, 2026

Copy link
Copy Markdown
Contributor Author

Update:

  1. The control loop requires that the policy be any callable (e.g. a learned neural policy, an model predictive controller...) that accepts 4 arguments:
  • x_hat (an estimate of the filtered state, provided by the filter)
  • t_now the current time.
  • t_next the next time at which the dynamics will advance.
  • s internal state.

It must return

  • A control u, a jax.numpy.array.
  • A new state s.

If u is a distribution, will raise an error and instruct the user to sample from the distribution instead. Tutorial have been updated.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

4 participants